syms A2B B2C C2D D2E E2F D2G G2H %% 距离参数
syms q1 q2 a1 a2 %% 角度参数
syms yaw pitch roll %% 探头偏航角 俯仰角 翻滚角
syms xx yy zz %% 输入数据
syms sq1 cq1 sq2 cq2 sa2 ca2
L = DH(A2B,-B2C,0,q1)%%杆1托盘
L2 = DH(C2D,D2E,a2,q2) %%杆2底座
L3 = DH(E2F,0,0,0)%% 杆3悬臂
%%构建旋转矩阵

R_x = [1, 0, 0;
       0, cos(roll), -sin(roll);
       0, sin(roll), cos(roll)];

R_y = [cos(pitch), 0, sin(pitch);
        0, 1, 0;
       -sin(pitch), 0, cos(pitch)];

R_z = [cos(yaw), -sin(yaw), 0;
       sin(yaw), cos(yaw), 0;
        0, 0, 1];

R = R_y*R_x*R_z;
B = sym(eye(4, 4));%%构建符号矩阵
B(1:3, 1:3) = R;%三阶转四阶矩阵
L4 = L*L2*L3;
%%求解
L5 = [xx;yy;zz;1];
%%L6 = B*L5;
L7 =L4*L5;

SC_simplify=[sin(q1) cos(q1) sin(q2) cos(q2) sin(a2) cos(a2)];
% theta常见角度的正余弦变换
sc_simplify=[sq1 cq1 sq2 cq2 sa2 ca2];
T8 =L7;
SC_simplify_temp = SC_simplify;
sc_simplify_temp=sc_simplify;
  for j=1:1:6
      T8=subs(T8, SC_simplify_temp(j),sc_simplify_temp(j));%%替换代码
  end
display(L7)
display(T8)

display(L4)

%% 姿态角yaw 偏航 z轴旋转
%% theta_yaw_z = atctan2(r21,r11)
theta_yaw_z = atan2(L4(2,1),L4(1,1));
theta_yaw = theta_yaw_z;
%% 姿态角roll 翻滚 x轴旋转
%% theta_pitch_y = atctan2(-r31,开方(r32平方+r33平方)
theta_pitch_y = atan2(-L4(3,1),sqrt(L4(3,2)*L4(3,2)+L4(3,3)*L4(3,3)));
theta_pitch = theta_pitch_y;
%% 姿态角 pitch 阜阳角 y轴旋转 
%% theta_roll_x = arctan2(r32,r33)
theta_roll_x = atan2(-L4(3,2),L4(3,3));
theta_roll= theta_roll_x;
  for j=1:1:6
      theta_yaw=subs(theta_yaw, SC_simplify_temp(j),sc_simplify_temp(j));%%替换代码
      theta_roll = subs(theta_roll, SC_simplify_temp(j),sc_simplify_temp(j));
      theta_pitch = subs(theta_pitch, SC_simplify_temp(j),sc_simplify_temp(j));
  end


display(theta_yaw);
display(theta_roll);
display(theta_pitch);

%%改进DH
function A = DH( a, d, alpha,theta)

A = [cos(theta)              -sin(theta)             0              a;
     sin(theta)*cos(alpha)  cos(theta)*cos(alpha)   -sin(alpha)    -d*sin(alpha);
     sin(theta)*sin(alpha)  cos(theta)*sin(alpha)   cos(alpha)     d*cos(alpha);
     0                      0                       0              1];      
%%  https://blog.csdn.net/kaisen_CN/article/details/129270094
end